#include "../header/FaceDetect.h"

using namespace std;

using namespace cv;


LBFRegressor regressor;

int detect_face_and_align(string first_file_path, string second_file_path){

    CvCapture* capture = 0;
    Mat frame, frameCopy;
    bool tryflip = false;
    double scale  = 1.0;
    CascadeClassifier cascade;

    Mat image = imread( first_file_path, 1 );
    if (image.empty()){
        cerr << __FILE__ << __FUNCTION__ << __LINE__ << endl;
        cerr << "read image fail" << endl;
        return -1;
    }
    Mat image2 = imread( second_file_path );
    if(image2.empty())
    {
        cerr << __FILE__ << __FUNCTION__ << __LINE__ << endl;
        cerr << "read image fail" << endl;
        return -1;
    }

    // -- 0. Load LBF model

    regressor.Load( modelPath );

    // -- 1. Load the cascades
//    if( !cascade.load( cascadeName ) ){
//        cerr << "ERROR: Could not load classifier cascade" << endl;
//        return -1;
//    }

    std::vector<cv::Rect> faces;

//    detect_by_seetface(image,faces);

    Face face,face2;
//    Mat img_proc ;
    if( detect_and_draw( image, cascade,regressor,face,scale, tryflip ) < 0)
    {
        cerr << __FILE__ << __FUNCTION__ << __LINE__ << endl;
        cerr << "ERROR: Found 0 Face (1)" << endl;
        return -1;
    }
    face.GetOutLine(image);
    face.get_head_img(image);

//    Mat img_proc2;
    if( detect_and_draw( image2, cascade,regressor,face2,scale, tryflip ) < 0 )
    {
        cerr << __FILE__ << __FUNCTION__ << __LINE__ << endl;
        cerr << "ERROR: Found 0 Face (2)" << endl;
        return -1;
    }
    face2.GetOutLine(image2);
    face2.get_head_img(image2);

//    cv::polylines(image,face.getFacePts(),true,cv::Scalar(255,0,0));

//    cv::polylines(image2,face2.getHead_pts(),true,cv::Scalar(255,0,0));

//    cv::imshow("head",image);
//    cv::imshow("head2",image2);
//    waitKey(0);

    cv::Size newsize = cv::Size( face2.getFace_bound().width * face.getHead_bound().width / face.getFace_bound().width,
                                 face2.getFace_bound().width * face.getHead_bound().height / face.getFace_bound().width );

    int centerw = face2.getFace_bound().x + face2.getFace_bound().width/2;

    Mat nsImg , mask_resize_img;
    cv::resize(face.getHead_img(),nsImg,newsize);
    cv::resize(face.getMask_img(),mask_resize_img,newsize);

    int h0 = face2.getHead_bound().y + face2.getHead_bound().height - newsize.height;
    int w0 = centerw - newsize.width / 2;

    Mat dst = Mat( image2.size(), image2.type()  );

    image2.copyTo(dst);

    Mat face2_mask_img = face2.fill_bg(dst);

    face2.setMask_img(face2_mask_img);

//    for (int h = 0; h < face2.getMask_img().rows; ++h) {
//        for (int w = 0; w < face2.getMask_img().cols; ++w) {
//            if( *face2.getMask_img().ptr<uchar>(h,w) == 255 )
//            {
//                cv::circle( dst, Point( w + face2.getHead_bound().x , h + face2.getHead_bound().y ) , 0 , Scalar(0,0,192) );
//            }
//        }
//    }


//    for (int h = 0; h < mask_resize_img.rows; ++h) {
//        for (int w = 0; w < mask_resize_img.cols; ++w) {
//            if( *mask_resize_img.ptr<uchar>(h,w) == 255 )
//            {
//                cv::circle( dst, Point( w + w0 , h + h0 ) , 1 , Scalar(0,128,128,30) );
//            }
//        }
//    }

//    cv::imshow("dst",dst);

//    imshow("img",img_proc2);

    nsImg.copyTo(dst(cv::Rect(w0,h0,newsize.width,newsize.height)),mask_resize_img);

    Mat inMask = Mat::zeros( dst.size(), CV_8U  );

    face2.gen_mask_img( inMask, Point(w0,h0), mask_resize_img );


    Mat in_paint_img/* = Mat( dst.size(), dst.type() )*/;

    inpaint(dst,inMask,in_paint_img,3,CV_INPAINT_TELEA);

    cv::imwrite(out_path,in_paint_img);

//    cv::resize(in_paint_img,in_paint_img,cv::Size(in_paint_img.cols/2,in_paint_img.rows/2));

//    cv::imshow("in_paint_img",in_paint_img);

//    for (int h = 0; h < inMask.rows; ++h) {
//        for (int w = 0; w < inMask.cols; ++w) {
//            if( *inMask.ptr<uchar>(h,w) == 255 )
//            {
//                cv::circle( dst , Point( w , h ) , 0 , Scalar(0,128,0) );
//            }
//        }
//    }

//    cv::imshow("dst",dst);


//    for (int h = 0; h < face2.getMask_img().rows; ++h) {
//        for (int w = 0; w < face2.getMask_img().cols; ++w) {
//            if( *face2.getMask_img().ptr<uchar>(h,w) == 255 )
//            {
//                cv::circle( img_proc2, Point( w + face2.getHead_bound().x , h + face2.getHead_bound().y ) , 1 , Scalar(0,0,255) );
//            }
//        }
//    }

//    for (int h = 0; h < mask_resize_img.rows; ++h) {
//        for (int w = 0; w < mask_resize_img.cols; ++w) {
//            if( *mask_resize_img.ptr<uchar>(h,w) == 255 )
//            {
//                cv::circle( img_proc2, Point( w + w0 , h + h0 ) , 1 , Scalar(0,0,255,30) );
//            }
//        }
//    }


//    for (int h = 0; h < inMask.rows; ++h) {
//        for (int w = 0; w < inMask.cols; ++w) {
//            if( *inMask.ptr<uchar>(h,w) == 255 )
//            {
//                cv::circle( img_proc2, Point( w , h ) , 1 , Scalar(0,0,255,30) );
//            }
//        }
//    }

//    cv::polylines( img_proc2,face2.getHead_pts(),true,cv::Scalar(255,0,0));

//    imshow("result",img_proc2);


//    waitKey(0);
//
//    cvDestroyWindow("model");
//    cvDestroyWindow("result");

    return 0;
}


int detect_by_opencv(const Mat & img, std::vector<cv::Rect> & faces , double scale)
{
    CascadeClassifier cascadeClassifier;
    // -- 1. Load the cascades
    if( !cascadeClassifier.load( cascadeName ) ){
        cerr << "ERROR: Could not load classifier cascade" << endl;
        return -1;
    }

    Mat gray, smallImg( cvRound (img.rows/scale), cvRound(img.cols/scale), CV_8UC1 );

    cvtColor( img, gray, CV_BGR2GRAY );
    resize( gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR );
    equalizeHist( smallImg, smallImg );

    cascadeClassifier.detectMultiScale( smallImg, faces,
                              1.1, 2, 0
                                      //                                      |CV_HAAR_FIND_BIGGEST_OBJECT
                                      //                                      |CV_HAAR_DO_ROUGH_SEARCH
                                      |CV_HAAR_SCALE_IMAGE,
                              Size(50, 50), Size(500,500) );

    if(faces.size() == 0)
        return -1;
    return 0;
}


int detect_and_draw( Mat& img, CascadeClassifier& cascade,
                    LBFRegressor& regressor, Face & face,
                    double scale, bool tryflip ){
    int i = 0;
    int64 t1 = 0,t2 = 0;
    vector<cv::Rect> faces,faces2;
//    const static Scalar colors[] =  { CV_RGB(0,0,255),
//                                      CV_RGB(0,128,255),
//                                      CV_RGB(0,255,255),
//                                      CV_RGB(0,255,0),
//                                      CV_RGB(255,128,0),
//                                      CV_RGB(255,255,0),
//                                      CV_RGB(255,0,0),
//                                      CV_RGB(255,0,255)} ;

    Mat gray;

    detect_by_seetface(img,faces,gray);

    if(faces.size() == 0)
        return -1;

//    printf( "detection time = %g ms\n", (t2 - t1)  / ( cvGetTickFrequency() * 1000. ) );

//    cout << "face count: " << faces.size() << endl;

    // --Alignment
    for( vector<cv::Rect>::const_iterator r = faces.begin(); r != faces.end(); r++, i++ ){
        Point center;

//        Scalar color = colors[i%8];

        BoundingBox boundingbox;

        boundingbox.start_x = r->x*scale;
        boundingbox.start_y = r->y*scale;
        boundingbox.width   = (r->width-1)*scale;
        boundingbox.height  = (r->height-1)*scale;
        boundingbox.centroid_x = boundingbox.start_x + boundingbox.width / 2.0;
        boundingbox.centroid_y = boundingbox.start_y + boundingbox.height / 2.0;

        t1 = cvGetTickCount();
        Mat1d current_shape = regressor.Predict(gray,boundingbox,1);
        t2 = cvGetTickCount() ;

//        printf( "alignment time = %g ms\n",  (t2 - t1)  / ( cvGetTickFrequency() * 1000. ) );

        // draw bounding box
//        rectangle(img,
//                  cvPoint( (int)boundingbox.start_x, (int)boundingbox.start_y),
//                  cvPoint( (int)(boundingbox.start_x+boundingbox.width), (int)(boundingbox.start_y+boundingbox.height)),
//                  colors[3], 1, 8, 0);

//        CvFont cvFont1 = cvFont(1.0);
//        for(int j = 0;j < global_params.landmark_num;j++){
//            circle(img,Point2d(current_shape(j,0),current_shape(j,1)),3,Scalar(255,255,0)/*,-1,8,0*/);
//            putText(img,to_string(j),Point2d(current_shape(j,0),current_shape(j,1)),cvFont1.font_face,cvFont1.vscale,colors[5]);
//        }

        face.get_chin_eyebrow(current_shape);
    }

    return 0;
}


int detect_by_seetface(const Mat &img, std::vector<cv::Rect> &faces, cv::Mat & gallery_img_gray) {
    // Initialize face detection model
    seeta::FaceDetection detector( fd_model.c_str() );
    detector.SetMinFaceSize(20);
    detector.SetScoreThresh(2.f);
    detector.SetImagePyramidScaleFactor(0.8f);
    detector.SetWindowStep(4, 4);

    //先将彩色图转换为灰色图
    cv::cvtColor(img, gallery_img_gray, CV_BGR2GRAY);

//    ImageData gallery_img_data_color(img.cols, img.rows, img.channels());
//    gallery_img_data_color.data = img.data;

    ImageData gallery_img_data_gray(gallery_img_gray.cols, gallery_img_gray.rows, gallery_img_gray.channels());
    gallery_img_data_gray.data = gallery_img_gray.data;

    // Detect faces
    std::vector<seeta::FaceInfo> gallery_faces = detector.Detect(gallery_img_data_gray);
    int32_t gallery_face_num = static_cast<int32_t>(gallery_faces.size());

    if(gallery_face_num == 0)
        return -1;

    int index = 0;
    double score = 0;
    for (int i = 0; i < gallery_faces.size(); ++i) {
        if( gallery_faces[i].score > score )
        {
            score = gallery_faces[i].score;
            index = i;
        }
    }

    faces.push_back( cv::Rect( gallery_faces[index].bbox.x, gallery_faces[index].bbox.y,
                               gallery_faces[index].bbox.width,gallery_faces[index].bbox.height) );

    return 0;
}


void detectAndDraw( Mat& img, CascadeClassifier& cascade,
                    LBFRegressor& regressor,
                    double scale, bool tryflip ){
    int i = 0;
    double t = 0;
    vector<cv::Rect> faces,faces2;
    const static Scalar colors[] =  { CV_RGB(0,0,255),
                                      CV_RGB(0,128,255),
                                      CV_RGB(0,255,255),
                                      CV_RGB(0,255,0),
                                      CV_RGB(255,128,0),
                                      CV_RGB(255,255,0),
                                      CV_RGB(255,0,0),
                                      CV_RGB(255,0,255)} ;
    Mat gray, smallImg( cvRound (img.rows/scale), cvRound(img.cols/scale), CV_8UC1 );

    cvtColor( img, gray, CV_BGR2GRAY );
    resize( gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR );
    equalizeHist( smallImg, smallImg );

    // --Detection
    t = (double)cvGetTickCount();
    cascade.detectMultiScale( smallImg, faces,
                              1.1, 2, 0
                                      //|CV_HAAR_FIND_BIGGEST_OBJECT
                                      //|CV_HAAR_DO_ROUGH_SEARCH
                                      |CV_HAAR_SCALE_IMAGE
            ,
                              Size(30, 30) );
    if( tryflip ){
        flip(smallImg, smallImg, 1);
        cascade.detectMultiScale( smallImg, faces2,
                                  1.1, 2, 0
                                          //|CV_HAAR_FIND_BIGGEST_OBJECT
                                          //|CV_HAAR_DO_ROUGH_SEARCH
                                          |CV_HAAR_SCALE_IMAGE
                ,
                                  Size(30, 30) );
        for( vector<cv::Rect>::const_iterator r = faces2.begin(); r != faces2.end(); r++ )
        {
            faces.push_back(cv::Rect(smallImg.cols - r->x - r->width, r->y, r->width, r->height));
        }
    }
    t = (double)cvGetTickCount() - t;
    printf( "detection time = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );

    // --Alignment
    t =(double)cvGetTickCount();
    for( vector<cv::Rect>::const_iterator r = faces.begin(); r != faces.end(); r++, i++ ){
        Point center;
        Scalar color = colors[i%8];
        BoundingBox boundingbox;

        boundingbox.start_x = r->x*scale;
        boundingbox.start_y = r->y*scale;
        boundingbox.width   = (r->width-1)*scale;
        boundingbox.height  = (r->height-1)*scale;
        boundingbox.centroid_x = boundingbox.start_x + boundingbox.width/2.0;
        boundingbox.centroid_y = boundingbox.start_y + boundingbox.height/2.0;

        t =(double)cvGetTickCount();
        Mat_<double> current_shape = regressor.Predict(gray,boundingbox,1);
        t = (double)cvGetTickCount() - t;
        printf( "alignment time = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );
//        // draw bounding box
//        rectangle(img, cvPoint(boundingbox.start_x,boundingbox.start_y),
//                  cvPoint(boundingbox.start_x+boundingbox.width,boundingbox.start_y+boundingbox.height),Scalar(0,255,0), 1, 8, 0);
        // draw result :: red
        for(int j = 0;j < global_params.landmark_num;j++){
            circle(img,Point2d(current_shape(j,0),current_shape(j,1)),3,Scalar(255,255,255),-1,8,0);
        }
    }
    cv::imshow( "result", img );
//    char a = waitKey(0);
//    if(a=='s'){
//        save_count++;
//        imwrite(to_string(save_count)+".jpg", img);
//    }
}

int FaceDetectionAndAlignment(string cascadeName, string inputName){
//    string inputName;
    CvCapture* capture = 0;
    Mat frame, frameCopy, image;
    bool tryflip = false;
    double scale  = 1.3;
    CascadeClassifier cascade;

//    if (inputname!=NULL){
//        inputName.assign(inputname);
//    }
    
    // name is empty or a number
    if( inputName.empty() || (isdigit(inputName.c_str()[0]) && inputName.c_str()[1] == '\0') ){
        capture = cvCaptureFromCAM( inputName.empty() ? 0 : inputName.c_str()[0] - '0' );
        int c = inputName.empty() ? 0 : inputName.c_str()[0] - '0' ;
        if(!capture){
            cout << "Capture from CAM " <<  c << " didn't work" << endl;
            return -1;
        }
    }
    // name is not empty
    else if( inputName.size() ){
        if (inputName.find(".jpg")!=string::npos||inputName.find(".png")!=string::npos
            ||inputName.find(".bmp")!=string::npos){
            image = imread( inputName, 1 );
            if (image.empty()){
                cout << "Read Image fail" << endl;
                return -1;
            }
        }
        else if(inputName.find(".mp4")!=string::npos||inputName.find(".avi")!=string::npos
                ||inputName.find(".wmv")!=string::npos){
            capture = cvCaptureFromAVI( inputName.c_str() );
            if(!capture) cout << "Capture from AVI didn't work" << endl;
            return -1;
        }
    }
    // -- 0. Load LBF model
    LBFRegressor regressor;
    regressor.Load(modelPath+"LBF.model");
    
    // -- 1. Load the cascades
    if( !cascade.load( cascadeName ) ){
        cerr << "ERROR: Could not load classifier cascade" << endl;
        return -1;
    }

    // cvNamedWindow( "result", 1 );
    // -- 2. Read the video stream
    if( capture ){
        cout << "In capture ..." << endl;
        for(;;){
            IplImage* iplImg = cvQueryFrame( capture );
            frame = cvarrToMat(iplImg);
            if( frame.empty() )
                break;
            if( iplImg->origin == IPL_ORIGIN_TL )
                frame.copyTo( frameCopy );
            else
                flip( frame, frameCopy, 0 );

            detectAndDraw( frameCopy, cascade,regressor, scale, tryflip );

//            cout<<"asfd"<< endl;
            if( waitKey( 200 ) >= 0 )
                goto _cleanup_;
        }

        waitKey(0);

_cleanup_:
        cvReleaseCapture( &capture );
    }
    else{
       
        if( !image.empty() ){
            cout << "In image read" << endl;
            detectAndDraw( image, cascade,regressor,  scale, tryflip );
            waitKey(0);
        }
        else if( !inputName.empty() ){
            /* assume it is a text file containing the
            list of the image filenames to be processed - one per line */
            cout << "In image set model" << endl;
            FILE* f = fopen( inputName.c_str(), "rt" );
            if( f ){
                char buf[1000+1];
                while( fgets( buf, 1000, f ) ){
                    int len = (int)strlen(buf), c;
                    while( len > 0 && isspace(buf[len-1]) )
                        len--;
                    buf[len] = '\0';
                    cout << "file " << buf << endl;
                    image = imread( buf, 1 );    
                    if( !image.empty() ){
                        detectAndDraw(image, cascade,regressor,scale, tryflip );
                        c = waitKey(0);
                        if( c == 27 || c == 'q' || c == 'Q' )
                            break;
                    }
                    else{
                        cerr << "Aw snap, couldn't read image " << buf << endl;
                    }
                }
                fclose(f);
            }
        }
    }

    cvDestroyWindow("result");

    return 0;
}

